#!/bin/bash

set -euo pipefail

cd $(dirname $(readlink -f $0))
PATCH_ROOT=$(pwd)/patches

cd ..

# Helper: clone if missing, then optionally checkout ref and apply patch.
# Usage: clone_and_patch <repo-url> <dir> [<ref>] [<patch-file>]
clone_and_patch() {
    local url=$1 dir=$2 ref=${3:-} patch=${4:-}
    if [ -d "$dir" ]; then
        printf 'SKIP: %s already exists\n' "$dir"
        return 0
    fi

    printf 'CLONE: %s -> %s\n' "$url" "$dir"

    # clone (use -b only if ref looks like a branch/tag; if it's a commit we clone default and checkout)
    if [ -n "$ref" ] && ! [[ "$ref" =~ ^[0-9a-fA-F]{7,40}$ ]]; then
        git clone --depth 1 --branch "$ref" "$url" "$dir"
    else
        git clone "$url" "$dir"
    fi

    # if clone succeeded and ref is a commit-ish hash, checkout it
    if [ -n "$ref" ] && [[ "$ref" =~ ^[0-9a-fA-F]{7,40}$ ]]; then
        git -C "$dir" checkout "$ref"
    fi

    # apply patch only if provided and exists
    if [ -n "$patch" ]; then
        local p="$PATCH_ROOT/$patch"
        if [ -f "$p" ]; then
            printf 'PATCH: applying %s to %s\n' "$p" "$dir"
            git -C "$dir" apply --check "$p"
            git -C "$dir" apply "$p"
        else
            printf 'WARN: patch file %s not found; skipping patch for %s\n' "$p" "$dir" >&2
        fi
    fi
}


# clone & patch main repos

clone_and_patch https://github.com/moveit/moveit2_tutorials moveit2_tutorials humble
if [ ! -d "moveit2" ]; then
    vcs import --recursive < moveit2_tutorials/moveit2_tutorials.repos
# special patches
    printf 'PATCH: applying moveit2.patch to moveit2\n'
    git -C "moveit2" apply --check $PATCH_ROOT/moveit2.patch
    git -C "moveit2" apply $PATCH_ROOT/moveit2.patch
    printf 'PATCH: applying moveit_task_constructor.patch to moveit_task_constructor\n'
    git -C "moveit_task_constructor" apply --check $PATCH_ROOT/moveit_task_constructor.patch
    git -C "moveit_task_constructor" apply $PATCH_ROOT/moveit_task_constructor.patch
fi


# clone & patch dependencies

mkdir -p moveit_deps
cd moveit_deps

clone_and_patch https://github.com/wg-perception/object_recognition_msgs.git object_recognition_msgs ros2
clone_and_patch https://github.com/OctoMap/octomap_msgs octomap_msgs ros2
clone_and_patch https://github.com/ros/angles angles ros2
clone_and_patch https://github.com/ros/eigen_stl_containers eigen_stl_containers ros2
clone_and_patch https://github.com/ros-controls/ros2_control ros2_control humble
clone_and_patch https://github.com/ros/joint_state_publisher joint_state_publisher ros2
clone_and_patch https://github.com/ros/xacro xacro ros2
# clone_and_patch https://github.com/moveit/srdfdom srdfdom ros2   # managed by moveit2_tutorial
clone_and_patch https://github.com/moveit/warehouse_ros warehouse_ros ros2
# clone_and_patch https://github.com/PickNikRobotics/launch_param_builder launch_param_builder # managed by moveit2_tutorial
clone_and_patch https://github.com/ros-controls/control_msgs control_msgs humble
clone_and_patch https://github.com/ros-controls/control_toolbox control_toolbox humble
clone_and_patch https://github.com/ros-controls/ros2_controllers ros2_controllers humble
clone_and_patch https://github.com/ros-drivers/joystick_drivers joystick_drivers \
    ros2 joystick_drivers.patch
clone_and_patch https://github.com/PickNikRobotics/generate_parameter_library generate_parameter_library \
    90840422895a92f4b5b1c2351a436210922eebc0
clone_and_patch https://github.com/moveit/geometric_shapes geometric_shapes \
    ros2 geometric_shapes.patch
clone_and_patch https://github.com/moveit/random_numbers random_numbers ros2
clone_and_patch https://github.com/pantor/ruckig ruckig \
    v0.14.0 ruckig.patch
clone_and_patch https://github.com/PickNikRobotics/RSL RSL 1.2.0
clone_and_patch https://github.com/PickNikRobotics/cpp_polyfills cpp_polyfills \
    fdabedce1e4eecbb628a87ec45873ca7839432df
clone_and_patch https://github.com/ros-perception/vision_opencv vision_opencv humble
clone_and_patch https://github.com/ros/diagnostics diagnostics ros2-humble
clone_and_patch https://github.com/flexible-collision-library/fcl fcl \
    cb0221698a9e877144d6945026e5c3fceb10439f
clone_and_patch https://github.com/ros-controls/realtime_tools realtime_tools \
    humble realtime_tools.patch
clone_and_patch https://github.com/pal-robotics/backward_ros backward_ros \
    1.0.8 backward_ros.patch
clone_and_patch https://github.com/ros/filters filters ros2
clone_and_patch https://github.com/ros-drivers/ackermann_msgs ackermann_msgs ros2
clone_and_patch https://github.com/ros-controls/kinematics_interface kinematics_interface humble
# clone_and_patch https://github.com/ompl/ompl ompl "" "" 
# ompl needs special commit, submodules and patch
if [ ! -d ompl ]; then
    git clone https://github.com/ompl/ompl ompl
    git -C ompl checkout 6c125908f088f3ccdf75c7460fafb54d6b5dc3a8
    git -C ompl submodule update --init --recursive
    if [ -f "$PATCH_ROOT/ompl.patch" ]; then
	printf 'PATCH: $PATCH_ROOT/ompl.patch to ompl\n'
        git -C ompl apply --check "$PATCH_ROOT/ompl.patch"
        git -C ompl apply "$PATCH_ROOT/ompl.patch"
    else
	printf 'SKIP: cannot find patch for ompl!\n'
    fi
else
    printf 'SKIP: ompl already exists\n'
fi

clone_and_patch https://github.com/ros-planning/moveit_msgs.git moveit_msgs humble
# clone_and_patch https://github.com/ros-planning/moveit_resources.git moveit_resources humble # managed by moveit2_tutorial
clone_and_patch https://github.com/PickNikRobotics/graph_msgs graph_msgs ros2
clone_and_patch https://github.com/moveit/py_binding_tools py_binding_tools ros2
clone_and_patch https://github.com/OctoMap/octomap octomap v1.9.8

